#include "ros/ros.h"
#include "std_msgs/String.h"
#include "pix_control/pix_auto_control.h"
#include "control.h"
#include <ctime>

namespace pix {
Control::Control()
{
  ros::NodeHandle private_node("~");
  Control::dev_handler_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
  struct ifreq ifr;
  strcpy(ifr.ifr_name, "can0");
  ioctl(dev_handler_, SIOCGIFINDEX, &ifr); //指定can0设备，并获取设备索引

  struct sockaddr_can addr;
  addr.can_family = AF_CAN;  // 指定协议簇
  addr.can_ifindex = ifr.ifr_ifindex; // 设备索引
  int bind_res = bind(dev_handler_, (struct sockaddr *)&addr, sizeof(addr));
  if(bind_res < 0)
  {
    ROS_ERROR(">> bind  dev_handel error!\r\n");
    return;
  }
  if(dev_handler_ < 0)
  {
    ROS_ERROR(">> open  can device error!");
  }else{
    ROS_INFO(">> open can device sucess!");
  }
}

Control::~Control()
{
  close(dev_handler_);
}

void Control::run()
{
  pix_auto_control_sub_ = nh_.subscribe<pix_control::pix_auto_control>("pix_control", 5, &Control::ctrl_callback, this);
  ros::spin();
} 


void Control::ctrl_callback(const pix_control::pix_auto_control msg)
{
  //TODO: mutex_lock
  cmd_mutex_.lock();
  setlocale(LC_CTYPE, "zh_CN.utf8");
  send_init(189); // 前后轮使能
  // send_init(183); // 挡位
  // send_init(180);//制动

  send_frames_[0].data[0] &= 0;
  send_frames_[0].data[1] &= 0;
  send_frames_[0].data[2] &= 0;
  send_frames_[0].data[3] &= 0;
  send_frames_[0].data[4] &= 0;
  send_frames_[0].data[5] &= 0;
  send_frames_[0].data[6] &= 0;
  send_frames_[0].data[7] &= 0;
  // memset(sendData_u_, 0, 8);

  if(msg.speed_value <0 || msg.speed_value >600)
  {
    ROS_ERROR("speed over range");
    return;
  }else{
    send_frames_[0].data[0] = msg.speed_value & 0xff;
    send_frames_[0].data[1] = (msg.speed_value >> 8) & 0xff;
  }
  if(msg.brake_value < 0 || msg.brake_value > 1024)
  {
    ROS_ERROR("brake over range");
    return;
  }else {
    send_frames_[0].data[2] = msg.brake_value & 0xff;
    send_frames_[0].data[3] = (msg.brake_value >> 8) & 0xff;
  }
  if(!((msg.steering_value < 1024) &&( msg.steering_value > -1024)))
  {
  ROS_ERROR("steering over range");
    return;
  }else {
    send_frames_[0].data[4] = msg.steering_value & 0xff;
    send_frames_[0].data[5] = (msg.steering_value >> 8) & 0xff;
    // send_frames_[0].data[4] = 0x00;
    // send_frames_[0].data[5] = 0x04;
  }
  send_frames_[0].data[6] = send_frames_[0].data[6] | ((msg.gear_shift & 0x3));
  send_frames_[0].data[6] = send_frames_[0].data[6] | ((msg.mode_selection & 0x3) << 4);
  send_frames_[0].data[7] = send_frames_[0].data[7] | msg.is_turn_left;
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.is_brake << 1);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.is_turn_right << 2);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.is_EPB << 3);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.is_turn_on_headlamp << 4);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.switch_normal_advanced_mode << 5);
  // send_frames_[0].data[7] = send_frames_[0].data[7] | (1 << 5);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.switch_low_high_speed << 6);
  // send_frames_[0].data[7] = send_frames_[0].data[7] | (msg.switch_disable_enable_auto << 7);
  send_frames_[0].data[7] = send_frames_[0].data[7] | (1 << 7);
  usleep(20);
  send_frames_[0].can_id = 0x183;
  send_frames_[0].can_dlc= 8;
  // memcpy(send_frames_[0].data, sendData_u_, 8);
  int ret = 0;
  ret = write(dev_handler_, &send_frames_[0], sizeof(send_frames_[0]));
  if(ret <= 0)
  {
    ROS_ERROR("send message failed, error code: %d", ret);
  }else{
    // ROS_INFO("successful!");
    std::cout << "send[8]:  ";
    for(int i = 0; i < 8; i++){
      std::cout << std::hex <<int(send_frames_[0].data[i]) << " ";
    }
    std::cout << std::endl;
  }
  cmd_mutex_.unlock();
  // send_init(180); // 制动使能
  // // usleep(20);
  // send_init(187); // 转向使能
  // // usleep(20);
  // send_init(189); // 前后轮使能
  // // usleep(20);
}

int Control::send_init(int id){
  int ret = 0;
  switch(id){
    case 183:
      send_frames_[1].can_id = 0x183;
      send_frames_[1].can_dlc= 8;
      send_frames_[1].data[0] = 0;
      send_frames_[1].data[1] = 0;
      send_frames_[1].data[2] = 0;
      send_frames_[1].data[3] = 0;
      send_frames_[1].data[4] = 0;
      send_frames_[1].data[5] = 0;
      send_frames_[1].data[6] = 2;
      send_frames_[1].data[7] = 0x80;
      ret = write(dev_handler_, &send_frames_[1], sizeof(send_frames_[0]));
      if(ret <= 0)
      {
        // ROS_INFO(" error !!");
      }else{
        // ROS_INFO(" ok !!");
      }
      break;
    case 187:
      send_frames_[2].can_id = 0x187;
      send_frames_[2].can_dlc= 8;
      send_frames_[2].data[0] = 0x11;
      send_frames_[2].data[1] = 0;
      send_frames_[2].data[2] = 0;
      send_frames_[2].data[3] = 0;
      send_frames_[2].data[4] = 0;
      send_frames_[2].data[5] = 0;
      send_frames_[2].data[6] = 0;
      send_frames_[2].data[7] = 0;
      ret = write(dev_handler_, &send_frames_[2], sizeof(send_frames_[0]));
      if(ret <= 0)
      {
        // ROS_INFO(" error !!");
      }else{
        // ROS_INFO(" ok !!");
      }
        break;
    case 189:
      send_frames_[3].can_id = 0x189;
      send_frames_[3].can_dlc= 8;
      send_frames_[3].data[0] = 0x0F;
      send_frames_[3].data[1] = 0;
      send_frames_[3].data[2] = 0;
      send_frames_[3].data[3] = 0;
      send_frames_[3].data[4] = 0;
      send_frames_[3].data[5] = 0;
      send_frames_[3].data[6] = 0;
      send_frames_[3].data[7] = 0;
      ret = write(dev_handler_, &send_frames_[3], sizeof(send_frames_[0]));
      if(ret <= 0)
      {
        // ROS_INFO(" error !!");
      }else{
        // ROS_INFO(" ok !!");
      }
      break;
    case 180: // 制动使能
      send_frames_[4].can_id = 0x18a;
      send_frames_[4].can_dlc= 8;
      send_frames_[4].data[0] = 0;
      send_frames_[4].data[1] = 0;
      send_frames_[4].data[2] = 1;
      send_frames_[4].data[0] = 0;
      send_frames_[4].data[4] = 0;
      send_frames_[4].data[5] = 0;
      send_frames_[4].data[6] = 0;
      send_frames_[4].data[7] = 0;
      ret = write(dev_handler_, &send_frames_[4], sizeof(send_frames_[0]));
      if(ret <= 0)
      {
        // ROS_INFO(" error !!");
      }else{
        // ROS_INFO(" ok !!");
      }
      break;
  }
}
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "pix_control_node"); // 初始pix控制节点

  pix::Control control; // 初始化Contorl对象
  control.send_init(183); // 基础使能
  usleep(20);

  control.send_init(180); // 制动使能
  usleep(20);
  control.send_init(187); // 转向使能
  usleep(20);
  control.send_init(189); // 前后轮使能
  usleep(20);
  control.run();

  return 0;
}
